#include <emmintrin.h>
#include <stddef.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <float.h>

const char* dgemm_desc = "DGEMM - Joe Gomes and Sara Alspaugh";

#define BLOCKSIZE 200
#define REG_BLOCKSIZE 4
#define K_UNROLLING 64 

#define min(a,b) (((a)<(b))?(a):(b))

// Square DGEMM takes three matrices of dimension lda X lda
void square_dgemm( int lda, double *A, double *B, double *C)
{
	// Block each matrix - all blocks must fit in L2 cache
	for ( int i=0;i<lda;i+=BLOCKSIZE )
		for ( int j=0;j<lda;j+=BLOCKSIZE )
			for ( int k=0;k<lda;k+=BLOCKSIZE )
			{
				// Get the correct block dimension
				int M = min(BLOCKSIZE,lda-i);
				int N = min(BLOCKSIZE,lda-j);
				int K = min(BLOCKSIZE,lda-k);

				// We now form register blocks out of the cache block
				// K_UNROLLING will control how many operations are done on the
				// C_ij's until they are written back to the array
				for ( int i0=0;i0<M;i0+=REG_BLOCKSIZE)
					for ( int j0=0;j0<N;j0+=REG_BLOCKSIZE)
						for ( int k0=0;k0<K;k0+=K_UNROLLING)
						{
							// This will help us navigate through matrix values
							int C_start = i+j*lda+i0+j0*lda;
							int A_start = i+k*lda+i0+k0*lda;
							int B_start = k+j*lda+k0+j0*lda;
							// We could either have a full block, a matrix fringe block,
							// or a register block that doesn't fit into the cache block
							int M0 = min(lda-i-i0,min(REG_BLOCKSIZE,BLOCKSIZE-i0));
							int N0 = min(lda-j-j0,min(REG_BLOCKSIZE,BLOCKSIZE-j0));
							int K0 = min(lda-k-k0,min(K_UNROLLING,BLOCKSIZE-k0));
	
							// If this is not a full block, we will do the 3 nested loops
							if ( M0 != REG_BLOCKSIZE || N0 != REG_BLOCKSIZE || K0 != K_UNROLLING )
							{
								for ( int i00=0;i00<M0;i00++ )
									for ( int j00=0;j00<N0;j00++ )
									{
										double cij = C[C_start+i00+j00*lda];
										for ( int k00=0;k00<K0;k00++ )
										{
											cij += A[A_start+i00+k00*lda]*B[B_start+k00+j00*lda];
										}
										C[C_start+i00+j00*lda]=cij;
									}
							}
							// We know this is a full block, but if the matrix has odd dimension
							// We must use unaligned loads and stores
							else if ( lda%2 != 0 )
							{
								// Unaligned load of REG_BLOCKSIZExREG_BLOCKSIZE block of C into SIMD registers
								__m128d C1 = _mm_loadu_pd(C+C_start);
                                                	        __m128d C2 = _mm_loadu_pd(C+C_start+lda);
	                                                        __m128d C3 = _mm_loadu_pd(C+C_start+2);
        	                                                __m128d C4 = _mm_loadu_pd(C+C_start+2+lda);
                	                                        __m128d C5 = _mm_loadu_pd(C+C_start+2*lda);
                        	                                __m128d C6 = _mm_loadu_pd(C+C_start+3*lda);
                                	                        __m128d C7 = _mm_loadu_pd(C+C_start+2+2*lda);
                                        	                __m128d C8 = _mm_loadu_pd(C+C_start+2+3*lda);
					
								// Iterate over values of K_UNROLLING
								for ( int w=0;w<K_UNROLLING;w++)
								{
									// Load wth column of A in SIMD registers
									__m128d A1 = _mm_loadu_pd(A+A_start+w*lda);
									__m128d A2 = _mm_loadu_pd(A+A_start+2+w*lda);
									// Load wth row of B into SIMD registers
									__m128d B1 = _mm_load1_pd(B+B_start+w);
									__m128d B2 = _mm_load1_pd(B+B_start+w+lda);
									__m128d B3 = _mm_load1_pd(B+B_start+w+2*lda);
									__m128d B4 = _mm_load1_pd(B+B_start+w+3*lda);
	
									// Update values of C
									C1 = _mm_add_pd(C1,_mm_mul_pd(A1,B1));
                        	                                        C2 = _mm_add_pd(C2,_mm_mul_pd(A1,B2));
                                	                                C3 = _mm_add_pd(C3,_mm_mul_pd(A2,B1));
                                        	                        C4 = _mm_add_pd(C4,_mm_mul_pd(A2,B2));
                                                	                C5 = _mm_add_pd(C5,_mm_mul_pd(A1,B3));
                                                        	        C6 = _mm_add_pd(C6,_mm_mul_pd(A1,B4));
	                                                                C7 = _mm_add_pd(C7,_mm_mul_pd(A2,B3));
        	                                                        C8 = _mm_add_pd(C8,_mm_mul_pd(A2,B4));
								}
									// Store values of C
									_mm_storeu_pd(C+C_start,C1);
                                        	                        _mm_storeu_pd(C+C_start+lda,C2);
                                                	                _mm_storeu_pd(C+C_start+2,C3);
                                                        	        _mm_storeu_pd(C+C_start+2+lda,C4);
	                                                                _mm_storeu_pd(C+C_start+2*lda,C5);
        	                                                        _mm_storeu_pd(C+C_start+3*lda,C6);
                	                                                _mm_storeu_pd(C+C_start+2+2*lda,C7);
                        	                                        _mm_storeu_pd(C+C_start+2+3*lda,C8);
							}	
							// Full block of an even dim. matrix can be treated with aligned load/stores
							else
							{
                	                                        // Unaligned load of REG_BLOCKSIZExREG_BLOCKSIZE block of C into SIMD registers
                        	                                __m128d C1 = _mm_load_pd(C+C_start);                                                       
                                	                        __m128d C2 = _mm_load_pd(C+C_start+lda);
                                        	                __m128d C3 = _mm_load_pd(C+C_start+2);
                                                	        __m128d C4 = _mm_load_pd(C+C_start+2+lda);
	                                                        __m128d C5 = _mm_load_pd(C+C_start+2*lda);
        	                                                __m128d C6 = _mm_load_pd(C+C_start+3*lda);
                	                                        __m128d C7 = _mm_load_pd(C+C_start+2+2*lda);
                        	                                __m128d C8 = _mm_load_pd(C+C_start+2+3*lda);
	
        	                                                // Iterate over values of K_UNROLLING
                	                                        for ( int w=0;w<K_UNROLLING;w++)
                        	                                {
                                	                                // Load wth column of A in SIMD registers
                                        	                        __m128d A1 = _mm_load_pd(A+A_start+w*lda);
                                                	                __m128d A2 = _mm_load_pd(A+A_start+2+w*lda);
                                                        	        // Load wth row of B into SIMD registers
	                                                                __m128d B1 = _mm_load1_pd(B+B_start+w);
        	                                                        __m128d B2 = _mm_load1_pd(B+B_start+w+lda);
                	                                                __m128d B3 = _mm_load1_pd(B+B_start+w+2*lda);
                        	                                        __m128d B4 = _mm_load1_pd(B+B_start+w+3*lda);
	
        	                                                        // Update values of C
                	                                                C1 = _mm_add_pd(C1,_mm_mul_pd(A1,B1));
                        	                                        C2 = _mm_add_pd(C2,_mm_mul_pd(A1,B2));
                                	                                C3 = _mm_add_pd(C3,_mm_mul_pd(A2,B1));
                                        	                        C4 = _mm_add_pd(C4,_mm_mul_pd(A2,B2));
                                                	                C5 = _mm_add_pd(C5,_mm_mul_pd(A1,B3));
                                                        	        C6 = _mm_add_pd(C6,_mm_mul_pd(A1,B4));
	                                                                C7 = _mm_add_pd(C7,_mm_mul_pd(A2,B3));
        	                                                        C8 = _mm_add_pd(C8,_mm_mul_pd(A2,B4));
                	                                        }
                        	                                        // Store values of C
                                	                                _mm_store_pd(C+C_start,C1);
                                        	                        _mm_store_pd(C+C_start+lda,C2);
                                                	                _mm_store_pd(C+C_start+2,C3);
                                                        	        _mm_store_pd(C+C_start+2+lda,C4);
	                                                                _mm_store_pd(C+C_start+2*lda,C5);
        	                                                        _mm_store_pd(C+C_start+3*lda,C6);
                	                                                _mm_store_pd(C+C_start+2+2*lda,C7);
                        	                                        _mm_store_pd(C+C_start+2+3*lda,C8);
								
							}							

					}
			}
} 




